#include <machine/asmdefs.h>

ASM_HEADER

FNDEFN_BEGIN(derotation1_asm)

        /* load inI, inQ */
        lr              %vr0,%r8,0          ||          sli         %r15,%r15,1 # multiply phase update by 2
        lr              %vr1,%r8,32         ||          lirl        %r8,512
        lr              %vr2,%r9,0          ||          sli         %r8,%r8,12  # put 0x200000 in r8
        lr              %vr3,%r9,32

        /* set loop count based on 9th (last parameter) */
        lwl             %r9,%r1,40

        /* downsample inI, inQ   |   save registers, set stack */
        rshf1           %vr1,%vr0,7         ||          stw         %r0,%r1,-4
        rshf1           %vr3,%vr2,7         ||          stw	    %r4,%r1,-8  ||        ctsr            %r9,5
        stwul           %r1,%r1,-128
        addi            %r4,%r1,4

LOOP_DEROTATE_11:
        /* add counter to 0x200000   |   store cos from previous iter (first store is a dummy store) */
        add             %r9,%r14,%r8        ||          stuhu       %r3,%r4,2
        /* extract bits 30-21 (shift right by 21 plus mask) to create index */
        extri           %r9,%r9,30,21
        /* zero out LSB of index     |   store sin from previous iter (first store is a dummy store) */
        andi            %r9,%r9,-2          ||          stuhl       %r0,%r4,32
        /* add index to cos table */
        add             %r3,%r12,%r9
        /* add index to sin table    |   load val from cos table */
        add             %r0,%r13,%r9        ||          luh         %r3,%r3,0
        /* inc counter by phaseupd   |   load val from sin table     |       loop */
        add             %r14,%r14,%r15      ||          luh         %r0,%r0,0   ||  loop            0,%lc1,LOOP_DEROTATE_11

        /* final stores of cos, sin, then load as a vector   |   set up vector of 1s in vr1 */
        stuhu           %r3,%r4,2           ||          lir         %r9,2
        stuhl           %r0,%r4,32          ||          sli         %r9,%r9,15
        addil           %r4,%r1,8
        lr              %vr4,%r4,0          ||          ctsr        %r9,73
        lr              %vr5,%r4,32         ||          rbroad      %vr1,%ac1,0

        /* multiply inI, inQ by cos, sin vectors     |   reset stack, get old registers back */
        rmuls           %vr6,%vr0,%vr4      ||      	lw          %r1,%r1,0
        rmuls           %vr7,%vr2,%vr5      ||          lw          %r0,%r1,-4
        radds           %vr7,%vr6,%vr7      ||          lw          %r4,%r1,-8
        rsra            %vr7,%vr7,%vr1

        rmuls           %vr6,%vr2,%vr4      ||          str         %vr7,%r10,0
        rmuls           %vr7,%vr0,%vr5
        rsubfs          %vr7,%vr7,%vr6      ||          addi        %r8,%r14,0
        rsra            %vr7,%vr7,%vr1

        str             %vr7,%r11,0         ||          ji	    %jt0,8

FNDEFN_END(derotation1_asm)

